#include "state_fall.h"
#include "motor_control.h"
#include <stdio.h>

#define KP 3.0f
#define KD 1.6f

void fall_enter(void)
{
    printf("fall enter\n");

    param.kp = KP;
    param.kd = KD;
    param.torque = 0;
    param.velocity = 0;
    param.position = 0;
}

void fall_run(void)
{
    for (size_t i = 0; i < 8; i++)
    {
        motor_set_param(i, &param);
    }
}

void fall_exit(void)
{
    printf("fall exit\n");
}
